#include "inisafezone.hh"
#include <string>
#include <string.h>
#include <math.h>
#include "jkutil/configparser.h"
#include "config.h"

using namespace std;
using Util::ConfigParser;

int checkSafePlaneParameter(SafePlane plane)
{
    double d;
    PmCartesian lineP1P2, lineP1P3, lineP1SafePoint, norm;
    pmCartCartSub(&plane.planePoints[1].point, &plane.planePoints[0].point, &lineP1P2);
    pmCartCartSub(&plane.planePoints[2].point, &plane.planePoints[0].point, &lineP1P3);
    pmCartCartCross(&lineP1P2, &lineP1P3, &norm);
    pmCartCartSub(&plane.safePoint.point, &plane.planePoints[0].point, &lineP1SafePoint);
    d = fabs(norm.x * lineP1SafePoint.x + norm.y * lineP1SafePoint.y + norm.z * lineP1SafePoint.z) / sqrt(pow(norm.x, 2) + pow(norm.y, 2) + pow(norm.z, 2));
    if ((!norm.x && !norm.y && !norm.z) || d == 0)
    {
        return -1;  // 确定安全平面的三点与安全点有重合点,或者三点共线
    }
    return 0;
}

int calculateSafePlaneNormal(SafePlane* plane)
{
    PmCartesian lineP1P2, lineP1P3, norm;
    pmCartCartSub(&plane->planePoints[1].point, &plane->planePoints[0].point, &lineP1P2);
    pmCartCartSub(&plane->planePoints[2].point, &plane->planePoints[0].point, &lineP1P3);
    pmCartCartCross(&lineP1P2, &lineP1P3, &norm);
    pmCartUnit(&norm, &plane->norm.point);
    plane->d = -(plane->norm.point.x * plane->planePoints[0].point.x + plane->norm.point.y * plane->planePoints[0].point.y +
                 plane->norm.point.z * plane->planePoints[0].point.z);
    return 0;
}

class ZoneConfigParser
{
public:
    ZoneConfigParser(const char* cfgfile)
    {
        this->cfgPath = cfgfile;
        memset(&zone, 0, sizeof(zone));
        memset(&safePlan, 0, sizeof(safePlan));
    }
    int loadZone()
    {
        ConfigParser parser(this->cfgPath.c_str());
        if (!parser.is_valid())
        {
            return -1;
        }
        this->zone.openEnable = getInt(parser, "OpenEnable", 0);
        this->zone.autoEnable = getInt(parser, "AutoEnable", 0);
        this->zone.safeZoneMode = getInt(parser, "SafeZoneMode", 0);
        this->zone.elbow_pos_limit_enable = getInt(parser, "ElbowPosLimitEnable", 0);
        this->zone.wrist_pos_limit_enable = getInt(parser, "WristPosLimitEnable", 0);
        // read SafePlane
        int safe = 0;
        if (0 == parser.get("SafePlane.safe", safe))
        {
            this->zone.safe = safe;
            if (readSafePlane(parser, this->zone.zone.plane))
            {
                this->zone.type = SafeZoneType::PLANE;
                return 0;
            }
        }

        return 0;
    }
    int saveZone()
    {
        int ret = 0;
        ConfigParser parser(this->cfgPath.c_str());
        if (!parser.is_valid())
        {
            parser.create_file("SafeZone");
            //如果发生文件丢失，则全部平面modify全部置为-1，在平面信息写入过程中补全配置文件
            for (int i = 0; i < 6; i++) { this->zone.zone.plane[i].modify = -1; }
        }
        parser.add("OpenEnable", this->zone.openEnable);
        parser.add("AutoEnable", this->zone.autoEnable);
        parser.add("SafeZoneMode", this->zone.safeZoneMode);
        parser.add("ElbowPosLimitEnable", this->zone.elbow_pos_limit_enable);
        parser.add("WristPosLimitEnable", this->zone.wrist_pos_limit_enable);
        parser.add("SafePlane.safe", this->zone.safe);
        ret = this->writeSafePlane(parser, this->zone.zone.plane);
        return ret;
    }

    void setZone(SafeZone zone) { this->zone = zone; }
    SafeZone getZone() { return this->zone; }
    void setSafePlane(SafePlane& safePlane) { this->safePlan = safePlan; }
    SafePlane getSafePlane() { return this->safePlan; }

private:
    void setPoint(ConfigParser& p, string exp, CartesianPos pose)
    {
        p.add(exp + ".x", pose.point.x, true);
        p.add(exp + ".y", pose.point.y, true);
        p.add(exp + ".z", pose.point.z, true);
        p.add(exp + ".a", pose.rpy.y, true);
        p.add(exp + ".b", pose.rpy.p, true);
        p.add(exp + ".c", pose.rpy.r, true);
    }
    CartesianPos getPoint(ConfigParser& p, string exp)
    {
        CartesianPos pose;
        memset(&pose, 0, sizeof(pose));
        p.get(exp + ".x", pose.point.x, true);
        p.get(exp + ".y", pose.point.y, true);
        p.get(exp + ".z", pose.point.z, true);
        p.get(exp + ".a", pose.rpy.y, true);
        p.get(exp + ".b", pose.rpy.p, true);
        p.get(exp + ".c", pose.rpy.r, true);
        return pose;
    }
    int getInt(ConfigParser& p, string exp, int defaultVal, bool attr = false)
    {
        int ret = 0;
        if (p.get(exp, ret, attr))
        {
            ret = defaultVal;
        }
        return ret;
    }
    int readSafePlane(ConfigParser& p, SafePlane* plane)
    {
        int cnt = 0;
        for (cnt = 0; cnt < SAFE_PLANE_MAX_CNT; cnt++)
        {
            auto planeItem = plane + cnt;
            string exp = ConfigParser::str_append_index("SafePlane.Plane", cnt);
            string name;
            if (p.get(exp + ".name", name, true))
            {
                memset(planeItem->name, 0, sizeof(planeItem->name));
                planeItem->enable = 0;
                break;
            }
            strncpy(planeItem->name, name.c_str(), sizeof(planeItem->name));
            p.get(exp + ".enable", planeItem->enable);
            p.get(exp + ".safeDist", planeItem->safeD);
            for (int i = 0; i < 3; i++)
            {
                string expPoint = ConfigParser::str_append_index(exp + ".point", i);
                planeItem->planePoints[i] = getPoint(p, expPoint);
            }
            planeItem->safePoint = getPoint(p, exp + ".safePoint");
            calculateSafePlaneNormal(planeItem);
        }
        return cnt;
    }
    int writeSafePlane(ConfigParser& p, SafePlane* plane)
    {
        for (int i = 0; i < SAFE_PLANE_MAX_CNT; i++)
        {
            if (0 == plane[i].modify)
            {
                continue;
            }
            if (checkSafePlaneParameter(plane[i]) && plane[i].modify != -1)
            {
                return -1;
            }
            string exp = ConfigParser::str_append_index("SafePlane.Plane", i);
            string planeName = plane[i].name;
            if (planeName == "")
            {
                planeName = "plane" + to_string(i + 1);
            }
            p.add(exp + ".name", planeName, true);
            p.add(exp + ".enable", plane[i].enable);
            for (int j = 0; j < 3; j++)
            {
                string pointExp = ConfigParser::str_append_index(exp + ".point", j);
                setPoint(p, pointExp, plane[i].planePoints[j]);
            }
            setPoint(p, exp + ".safePoint", plane[i].safePoint);
            p.add(exp + ".safeDist", plane[i].safeD);
        }
        return 0;
    }

private:
    string cfgPath;
    SafeZone zone;
    SafePlane safePlan;
};

int SafeZoneConfig::loadSafeZone(const char* cfgfile, SafeZone& zone)
{
    ZoneConfigParser zoneConfigParser(cfgfile);
    int ret = zoneConfigParser.loadZone();
    if (ret)
    {
        memset(&zone, 0, sizeof(zone));
        return ret;
    }
    zone = zoneConfigParser.getZone();
    return 0;
}
int SafeZoneConfig::saveSafeZone(const char* cfgfile, SafeZone zone)
{
    ZoneConfigParser zoneConfigParser(cfgfile);
    zoneConfigParser.setZone(zone);
    int ret = zoneConfigParser.saveZone();
    return ret;
}

int SafeZoneConfig::loadSafeZoneEnableMode(const char* cfgfile, int& enableMode) { return 0; }
int SafeZoneConfig::saveSafeZoneEnableMode(const char* cfgfile, int enableMode) { return 0; }
int SafeZoneConfig::loadSafeZoneActiveMode(const char* cfgfile, int& activeMode) { return 0; }
int SafeZoneConfig::saveSafeZoneActiveMode(const char* cfgfile, int activeMode) { return 0; }
int SafeZoneConfig::loadSafeZoneLimitMode(const char* cfgfile, int& limitMode) { return 0; }
int SafeZoneConfig::saveSafeZoneLimitMode(const char* cfgfile, int limitMode) { return 0; }
int SafeZoneConfig::loadSafePlane(const char* cfgfile, SafePlane& safePlane) { return 0; }
int SafeZoneConfig::saveSafePlane(const char* cfgfile, SafePlane SafePlane) { return 0; }
